#include "camera.h"
#include <unistd.h>
#include <chrono>

cv::VideoCapture cap;

double kp = 0;
double ki = 0;
double kd = 0;

int screenWidth, screenHeight;
int newWidth, newHeight;
int fb;

static std::chrono::steady_clock::time_point turn_start_time;
static std::chrono::steady_clock::time_point obstacle_time;
static std::chrono::steady_clock::time_point obstacle_after_time;
// 创建帧缓冲区
uint16_t *fb_buffer;
PwmController servo(1, 0);

#define calc_scale 2


/*
@​​ params​：
camera_id：摄像头设备编号（0对应/dev/video0,）。
dest_fps：目标帧率（FPS）。
width/height：摄像头捕获图像的分辨率（如 320x240）。

​​@ return​​：
成功：返回每帧的理想处理时间（毫秒）。
失败：返回 -1。
*/
int CameraInit(uint8_t camera_id, double dest_fps, int width, int height)
{

    servo.setPeriod(3000000);       // 设置 PWM 周期为 3ms（适用于舵机控制）
    servo.setDutyCycle(1600000);    // 初始占空比 1.6ms（舵机中位）
    servo.enable();                 // 启用 PWM 输出


    /*Linux ​​帧缓冲区（Framebuffer）​​是操作系统提供的一种抽象层，用于​直接操作显示设备（如屏幕）的显存​​。
    它的核心作用是 ​​图像数据直接输出到屏幕​​，常见于嵌入式系统.
    帧缓冲区可以在​​无桌面环境​​下直接显示图像。Linux 内核为所有显示设备（LCD、HDMI、VGA、DP 等）抽象出一个标准接口：/dev/fb0、/dev/fb1 等。
    无论你的显示器是 ​​LCD 屏、HDMI 外接显示器，还是 VGA 老式显示器​​，应用程序都只需要操作 /dev/fb0，而 ​​不需要关心底层硬件差异​​。*/
    // 打开帧缓冲区设备

    fb = open("/dev/fb0", O_RDWR); // 打开 Linux 帧缓冲区设备
    if (fb == -1)
    {
        std::cerr << "无法打开帧缓冲区设备" << std::endl;
        return -1;
    }

    // 获取帧缓冲区设备信息
    // 通过 ioctl系统调用获取​帧缓冲区的可变信息​​，存储到 struct fb_var_screeninfo结构体中。
    // 这些信息描述了当前显示设备的​分辨率、像素格式、时序参数​​等关键属性
    // 结构体fb_var_screeninfo定义在 Linux 内核头文件 <linux/fb.h>中，主要包含以下字段:
    // 1.xres, yres:当前可见分辨率（宽度 x 高度，单位：像素）  当前屏幕 ​​实际显示​​ 的像素区域（即可见部分）
    // 2.xres_virtual, yres_virtual:虚拟分辨率（可能大于实际分辨率，用于双缓冲等优化）
    // 3.bits_per_pixel:每个像素的位数（色深），例如 16（RGB565）、24（RGB888）、32（ARGB8888）

    struct fb_var_screeninfo vinfo;
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
    {
        std::cerr << "无法获取帧缓冲区信息" << std::endl;
        close(fb);
        return -1;
    }

    // 动态设置屏幕分辨率
    screenWidth = vinfo.xres;
    screenHeight = vinfo.yres;


    // 虚拟分辨率（xres_virtual/yres_virtual）可能​大于实际分辨率​​（xres/yres）,这是为了预留额外空间存储未显示的帧，避免撕裂​(​双缓冲/三缓冲​​)
    // 计算帧缓冲区大小  fb_size​​ 表示整个帧缓冲区占用的​字节数
    // fb_size包含​所有虚拟分辨率区域的像素数据​​，但只有xres*yres的部分实际显示
    size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8; // ​​除以 8​​：将位数转换为字节数（1字节 = 8位）



    // mmap将显存映射到用户空间​
    /*参数解析:
    NULL：由内核自动选择映射地址。
    fb_size：需要映射的内存大小。
    PROT_READ | PROT_WRITE：映射区域可读写。
    MAP_SHARED：修改会同步到显存（直接影响屏幕）。
    fb：帧缓冲区设备文件描述符。
    0：偏移量（从显存起始位置开始映射）。

    用uint16_t是因为:vinfo.bits_per_pixel表示每个像素占用的位数,而此屏幕使用的是RGB565格式（16位, 嵌入式设备常用）
    当 bits_per_pixel=16时，每个像素恰好占用 ​​2字节（16位）​​，与 uint16_t类型完全匹配。
    */
    // fb_buffer的本质​​：一个指向​显存映射区域​​的指针，类型为 uint16_t*。​​可以直接修改这个数组来改变屏幕显示内容​
    // 每个像素按​行优先顺序​​连续存储, 例如设置坐标 (x, y) 的像素为红色（RGB565: 0xF800）：fb_buffer[y * vinfo.xres_virtual + x] = 0xF800;
    fb_buffer = (uint16_t *)mmap(NULL, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0);
    if (fb_buffer == MAP_FAILED)
    {
        std::cerr << "无法映射帧缓冲区到内存" << std::endl;
        close(fb);
        return -1;
    }

    // 打开默认摄像头（设备编号 0）
    cap.open(camera_id);

    // 检查摄像头是否成功打开
    if (!cap.isOpened())
    {
        printf("无法打开摄像头\n");
        munmap(fb_buffer, fb_size);
        close(fb);
        return -1;
    }

    cap.set(cv::CAP_PROP_FRAME_WIDTH, width);                                  // 宽度
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, height);                                // 高度
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')); // 视频流格式
    cap.set(cv::CAP_PROP_AUTO_EXPOSURE, -1);    
    //cap.set(cv::CAP_PROP_AUTO_EXPOSURE, 0);  // 关闭自动曝光
    //cap.set(cv::CAP_PROP_EXPOSURE, -4);      // 手动设置曝光值（具体值需实验）                               // 设置自动曝光

    // 获取摄像头实际分辨率
    int cameraWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    int cameraHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    printf("摄像头分辨率: %d x %d\n", cameraWidth, cameraHeight); // 640X480

    // 计算宽高方向的缩放比例​，确保图像适应屏幕
    double widthRatio = static_cast<double>(screenWidth) / cameraWidth; // 屏幕宽 / 摄像头图像宽
    double heightRatio = static_cast<double>(screenHeight) / cameraHeight; // 屏幕高 / 摄像头图像高
    double scale = std::min(widthRatio, heightRatio); // 选择较小的比例，确保图像不超出屏幕

    newWidth = static_cast<int>(cameraWidth * scale);
    newHeight = static_cast<int>(cameraHeight * scale);
    printf("自适应分辨率: %d x %d\n", newWidth, newHeight); // 160X120

    // 计算帧率
    double fps = cap.get(cv::CAP_PROP_FPS);
    printf("Camera fps:%lf\n", fps);

    //  进一步缩小calc_scale倍, 可能是为了使计算量更小，适合实时处理
    line_tracking_width = newWidth / calc_scale; 
    line_tracking_height = newHeight / calc_scale;

    printf("line_tracking_width: %d\n", line_tracking_width); // 80
    printf("line_tracking_height: %d\n", line_tracking_height); // 60


    // 计算每帧的延迟时间（ms）
    return static_cast<int>(1000.0 / std::min(fps, dest_fps));
}

void cameraDeInit(void)
{
    cap.release();

    // 获取帧缓冲区设备信息
    struct fb_var_screeninfo vinfo;
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo) == -1)
    {
        std::cerr << "无法获取帧缓冲区信息" << std::endl;
    }
    else
    {
        // 计算帧缓冲区大小
        size_t fb_size = vinfo.yres_virtual * vinfo.xres_virtual * vinfo.bits_per_pixel / 8;

        // 取消映射
        munmap(fb_buffer, fb_size);
    }

    close(fb);
}

int saved_frame_count = 0;
bool saveCameraImage(cv::Mat frame, const std::string &directory)
{
    if (frame.empty())
    {
        std::cerr << "Save Error: Frame is empty." << std::endl;
        return 0;
    }

    // 构建文件名
    std::ostringstream filename;
    filename << directory << "/image_" << std::setw(5) << std::setfill('0') << saved_frame_count << ".jpg";

    saved_frame_count++;
    // 保存图像
    return cv::imwrite(filename.str(), frame);
}

std::mutex frameMutex;
cv::Mat pubframe;
bool streamCaptureRunning;
void streamCapture(void)
{
    cv::Mat frame;
    while (streamCaptureRunning)
    {
        cap.read(frame);
        frameMutex.lock();
        pubframe = frame;
        frameMutex.unlock();
    }
    return;
}

// 初始化PIDController类的实例，对象名为ServoControl  0.12, 0.0, 0.5   speed = 15
PIDController ServoControl(0.12, 0.0, 0.5, 0.0, POSITION, 1250000);

int CameraHandler(void)
{
    cv::Mat resizedFrame;

    frameMutex.lock();
    raw_frame = pubframe;
    frameMutex.unlock();

    if (raw_frame.empty()) // 检查是否成功捕获图像
    {
        printf("无法捕获图像\n");
        return -1;
    }

    if (readFlag(saveImg_file))
    {
        if (saveCameraImage(raw_frame, "./image"))
        {
            printf("图像%d已保存\n", saved_frame_count);
        }
        else
        {
            printf("图像保存失败\n");
            return -1;
        }
    }

    { // 图像计算
        image_main();
    }
    
    float measured_value = 0;

    if (readFlag(start_file))
    {   

        // 误差(error)计算，此误差传给舵机来控制舵机响应
        double error = 0.0;
        double total_weight = 0.0;
        
        // 计算权重总和（用于归一化）
        for (int row = 50; row >= 30; row--) {
            total_weight += row;  // 行数作为权重
        }

        for (int row = 50; row >= 30; row--) {
            if (row >= 0 && mid_line_filtered[row] != -1) {
                double weight = row / total_weight;  // 归一化权重
                error += (mid_line_filtered[row] * calc_scale - newWidth / 2) * weight;
            }
        }
        
        // 计算权重总和（用于归一化）
        /*
        for (int row = 50; row >= 30; row--) {
            // 基础权重为行号
            double base_weight = row;
            
            // 对50 > row > 40的范围增加额外权重
            if (row > 35 && row < 45) {
                base_weight *= 2.0; // 提高50%
            }
            
            total_weight += base_weight;
        }

        for (int row = 50; row >= 30; row--) {
            if (row >= 0 && mid_line_filtered[row] != -1) {
                // 计算当前行的权重（与上面相同的逻辑）
                double base_weight = row;
                if (row > 35 && row < 45) {
                    base_weight *= 2.0;
                }
                double weight = base_weight / total_weight;
                error += (mid_line_filtered[row] * calc_scale - newWidth / 2) * weight;
            }
        }*/

        if (total_weight > 0) {

            double servoduty = -ServoControl.update(error); // PID控制计算, 位于213行

            //std::cout << "error:" << error << std::endl;

            const double dead_zone = 2.0;
            //std::cout << "servoduty:" << servoduty << std::endl;
                                                        // -6.0 6.0

            servoduty = std::clamp(servoduty, -6.0, 6.0); // 输出限幅​.将PID输出限制在 [-5.5, 5.5]范围内，避免舵机因过大信号损坏或过度转向
            
            // 转换为舵机PWM信号​​
            // servo.readPeriod()​​：舵机PWM信号的周期（如代码中为 3000000 ns，即3ms，对应常见舵机控制频率）
            // 1520000​​：舵机中位对应的脉冲宽度（1.52ms，通常为0度或直行位置）
            // 将 servoduty（范围 [-5.5, 5.5]）按比例映射到脉冲宽度
            // servoduty=0→ 1520000 ns（中位）。
            // servoduty=5.5→ 1520000 + 5.5/100 * 3000000 = 1685000 ns（右转极限）。
            // servoduty=-5.5→ 1520000 - 5.5/100 * 3000000 = 1355000 ns（左转极限）
            double servoduty_ns = (servoduty) / 100 * servo.readPeriod() + 1520000; 


            servo.setDutyCycle(servoduty_ns);



            // 键盘控制车转向一定时间
            if (turn_left) {
                auto now = std::chrono::steady_clock::now();
                auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - turn_start_time).count();
    
                if (elapsed < 800) {
                    double servoduty_ns = (-5.0) / 100 * servo.readPeriod() + 1520000;
                    servo.setDutyCycle(servoduty_ns);
                    std::cout << "Turning left..." << std::endl;
                } else {
                    turn_left = false;
                }
            } else {
                turn_start_time = std::chrono::steady_clock::now();
            }

            if (obstacle_true) {
                auto now = std::chrono::steady_clock::now();
                auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - obstacle_time).count();
    
                if (elapsed < 600) {
                    double servoduty_ns = (-5.0) / 100 * servo.readPeriod() + 1520000;
                    servo.setDutyCycle(servoduty_ns);
                    std::cout << "obstacle_true..." << std::endl;
                } else if (elapsed > 600 && elapsed < 1000){
                    double servoduty_ns = (5.0) / 100 * servo.readPeriod() + 1520000;
                    servo.setDutyCycle(servoduty_ns);
                }else{
                    obstacle_true = false;
                }
            } else {
                obstacle_time = std::chrono::steady_clock::now();
            }

            if (obstacle_after_true) {
                auto now = std::chrono::steady_clock::now();
                auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - obstacle_after_time).count();
    
                if (elapsed < 200) {
                    double servoduty_ns = (-4.0) / 100 * servo.readPeriod() + 1520000;
                    servo.setDutyCycle(servoduty_ns);
                    std::cout << "obstacle_after_true..." << std::endl;
                } else {
                    obstacle_after_true = false;
                }
            } else {
                obstacle_after_time = std::chrono::steady_clock::now();
            }
        }

    }
    else
    {
        // servo.setDutyCycle(1520000);
    }

    // 显示图片
    if (readFlag(showImg_file))
    {
        cv::Mat fbImage(screenHeight, screenWidth, CV_8UC3, cv::Scalar(0, 0, 0));
        

        // 输出原始二值化图像的尺寸  80X60(宽X高)
        //std::cout << "原始二值化图像尺寸 (binarizedFrame): " 
        //  << binarizedFrame.cols << " x " << binarizedFrame.rows << std::endl;

        // 输出目标显示尺寸（newWidth 和 newHeight）160X120
        //std::cout << "目标显示尺寸 (newWidth x newHeight): " 
        //  << newWidth << " x " << newHeight << std::endl;

        // 缩放视频到新尺寸 
        //cv::resize(track, resizedFrame, cv::Size(newWidth, newHeight));
        cv::Mat binaryDisplay;
        if (track.empty()) {
            std::cerr << "二值化图像为空！" << std::endl;
            return -1;
        }
        if (track.type() != CV_8UC1) {
            cv::cvtColor(track, binaryDisplay, cv::COLOR_BGR2GRAY);
        } else {
            binaryDisplay = track.clone();
        }

        
        /*--------------------------------------显示处理后的黑白图像---------------------------------------*/
        // 调整二值图像大小（可选，如果原尺寸与目标尺寸不同）
        cv::Mat resizedBinary;
        cv::resize(binaryDisplay, resizedBinary, cv::Size(newWidth, newHeight), 0, 0, cv::INTER_NEAREST);

        // 输出缩放后的二值图像尺寸 160x120
        //std::cout << "缩放后的二值图像尺寸 (resizedBinary): " 
        //  << resizedBinary.cols << " x " << resizedBinary.rows << std::endl;

        // 将单通道二值图转换为3通道BGR格式（因为 fbImage 是彩色图）
        cv::Mat coloredBinary;
        cv::cvtColor(resizedBinary, coloredBinary, cv::COLOR_GRAY2BGR);

        // 将单通道的二值化图像转换为三通道的彩色图像
        //cv::Mat coloredResizedFrame;
        //cv::cvtColor(resizedFrame, coloredResizedFrame, cv::COLOR_GRAY2BGR); // 转换为彩色图像

        // 将缩放后的图像居中放置在帧缓冲区图像中，填充黑色边框
        fbImage.setTo(cv::Scalar(0, 0, 0)); // 清空缓冲区（填充黑色）
        cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);

        // 160x120
        //std::cout << "屏幕显示区域 (ROI): 位置 (" << roi.x << ", " << roi.y  
        //  << "), 尺寸 " << roi.width << " x " << roi.height << std::endl;
        coloredBinary.copyTo(fbImage(roi));
        /*---------------------------------------------------------------------------------------------*/



        /*-------------------------------显示原始图像raw_frame---------------------------------------------
        // 缩放原始图像到目标尺寸
        cv::resize(raw_frame, resizedFrame, cv::Size(newWidth, newHeight));

        // 将缩放后的图像居中放置在帧缓冲区图像中
        fbImage.setTo(cv::Scalar(0, 0, 0));  // 清空缓冲区（填充黑色）
        cv::Rect roi((screenWidth - newWidth) / 2, (screenHeight - newHeight) / 2, newWidth, newHeight);
        resizedFrame.copyTo(fbImage(roi));
        -----------------------------------------------------------------------------------------------*/


        // 绘制左右边界线和中线
        int scaledLeftX, scaledRightX, scaledMidX, scaledY;
        int foresee = readDoubleFromFile(foresee_file);
        for (int y = 30; y <= 50; y++)
        {
            // 根据缩放比例调整X坐标
            scaledLeftX = static_cast<int>(left_line_filtered[y] * calc_scale);
            scaledRightX = static_cast<int>(right_line_filtered[y] * calc_scale);
            scaledMidX = static_cast<int>(mid_line_filtered[y] * calc_scale);
            scaledY = static_cast<int>(y * calc_scale);

            // 绘制左边界（红）
            cv::line(fbImage(roi), cv::Point(scaledLeftX, scaledY), cv::Point(scaledLeftX, scaledY), cv::Scalar(0, 0, 255), calc_scale);
            // 绘制右边界（绿）
            cv::line(fbImage(roi), cv::Point(scaledRightX, scaledY), cv::Point(scaledRightX, scaledY), cv::Scalar(0, 255, 0), calc_scale);
            // 绘制中线  （蓝）
            cv::line(fbImage(roi), cv::Point(scaledMidX, scaledY), cv::Point(scaledMidX, scaledY), cv::Scalar(255, 0, 0), calc_scale);
        }

        // 将帧缓冲区图像转换为RGB565格式
        convertMatToRGB565(fbImage, fb_buffer, screenWidth, screenHeight);
    }
    return 0;
}
